#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>

class ImagePublisherNode : public rclcpp::Node
{
public:
    ImagePublisherNode() : Node("image_publisher_node")
    {
        // 创建发布者，发布主题为 /image
        publisher_ = this->create_publisher<sensor_msgs::msg::Image>("/airsim_node/drone_1/camera1/Scene", 10);

        // 使用 OpenCV 读取图像
        image = cv::imread("/home/nvidia/261.png", cv::IMREAD_COLOR);
        if(image.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Failed to load image");
            return;
        }

        // 设置定时器，周期性发布图像
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(1000), 
            [this]() { this->publish_image(image); }); // 使用闭包捕获 image
    }

private:
    void publish_image(const cv::Mat& image)
    {
        // 将 OpenCV 的 Mat 格式图像转换为 ROS2 的 Image 消息
        auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", image).toImageMsg();
        publisher_->publish(*msg);
        RCLCPP_INFO(this->get_logger(), "Image published");
    }

    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
     cv::Mat image;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImagePublisherNode>());
    rclcpp::shutdown();
    return 0;
}
